#include <test.hpp>
namespace ohe_framework{

int previous_y_;
int previous_x_;
double time;
Point first,second;
DynamicState previous_x,current_x,previous_y,current_y;
Detection state;

bool setupImage(std::vector<ImageMatrixUchar> &images, Point &roi_origin, Detection &state)
{
    Point roi_range(RANGE_ROWS,RANGE_COLS);
    switch(state.getCurrentState())
    {
        case START:
            resetCalulation();
            roi_origin.y = 0;
            roi_origin.x = 0;
            images[0].removeROI();
            images[1].removeROI();
            images[2].removeROI();
            //printDebugMessage("searching for wire");
            return false;
            break;
        case WIRE:
            previous_x.x = current_x.x;
            previous_x.u = current_x.u;
            current_x.x = previous_x.x+previous_x.u*time+previous_x.a*time*time/2.0;
            if((current_x.x) < roi_range.x/2.0)
                roi_origin.x = 0;
            else
                roi_origin.x=(unsigned int)((int)current_x.x-roi_range.x/2.0);
            if((previous_y.x) < roi_range.y/2.0)
                roi_origin.y = 0;
            else
                roi_origin.y=(unsigned int)((int)previous_y.x-roi_range.y/2.0);
            previous_y_ = abs(((int)previous_y.x-roi_origin.y));
            previous_x_ = abs(((int)current_x.x-roi_origin.x));
            //printDebugMessage("applying ROI");
            break;
        case WIRE_SECOND:
        //////// as above but with different conditions
            previous_x.x = current_x.x;
            previous_x.u = current_x.u;
            current_x.x = previous_x.x+previous_x.u*time+previous_x.a*time*time/2.0;
            if((current_x.x) < roi_range.x/2.0)
                roi_origin.x = 0;
            else
                roi_origin.x=(unsigned int)((int)current_x.x-roi_range.x/2.0);
            if((previous_y.x) < roi_range.y/2.0)
                roi_origin.y = 0;
            else
                roi_origin.y=(unsigned int)((int)previous_y.x-roi_range.y/2.0);
            previous_y_ = abs(((int)previous_y.x-roi_origin.y));
            previous_x_ = abs(((int)current_x.x-roi_origin.x));
            //printDebugMessage("applying ROI");
            break;
        case MAST:
            resetCalulation();
            roi_origin.y = 0;
            roi_origin.x = 0;
            images[0].removeROI();
            images[1].removeROI();
            images[2].removeROI();
            //printDebugMessage("waiting for mast to get over");
            return false;
            break;
        case OBJECTS:
        ////////  learn more about bridge;
            resetCalulation();
            roi_origin.y = 0;
            roi_origin.x = 0;
            images[0].removeROI();
            images[1].removeROI();
            images[2].removeROI();
            //printDebugMessage("waiting for bridge to get over");
            return false;
            break;
    }
    images[0].setROI(roi_origin,roi_range);
    images[1].setROI(roi_origin,roi_range);
    images[2].setROI(roi_origin,roi_range);
    return true;
}

void processImage(ImageMatrixUchar &color_image,
                  std::vector<Blob> &contours)
{
    ImageMatrixUchar gray_image, threshold_image;
    convertToGrayscale(color_image,gray_image);
    cv::Mat gray(cv::Size(color_image.getCols(),color_image.getRows()),CV_8UC1);
    cv::Mat thres(cv::Size(color_image.getCols(),color_image.getRows()),CV_8UC1);
    gray_image.matrixToMat(gray);
    cv::blur(gray,gray,cv::Size(5,5));
    //cv::imshow("Gray Image",gray);
    gray_image.matToMatrix(gray);
    grayscaleToBinary(gray_image,threshold_image,120);
    threshold_image.matrixToMat(thres);
    //cv::imshow("Threshold Image",thres);
    findContours(threshold_image,contours);
    gray_image.~ImageMatrixUchar();
    threshold_image.~ImageMatrixUchar();
    gray.~Mat();
}

bool computeAndSetROI();

void separateContours(std::vector<Blob> &contours,
                      std::vector<Blob> &small_wire,
                      std::vector<Blob> &wire,
                      std::vector<Blob> &mast,bool search)
{
    switch(state.getCurrentState())
    {
        case START:
            for(unsigned int i=0;i<contours.size();i++)
            {
                Point start,end;
                contours[i].getBoundingRectangle(start,end);
                double ratio = (double)(end.x-start.x)/(end.y-start.y);
                int area = (end.x-start.x)*(end.y-start.y);
                if((ratio > 10)&&(area>100))
                {
                    mast.push_back(contours[i]);
                }
                else
                {
                    if(area<20&&search)
                    {
                        small_wire.push_back(contours[i]);
                    }
                    else
                    {
                        wire.push_back(contours[i]);
                    }
                }
            }
            break;
        case WIRE:
            for(unsigned int i=0;i<contours.size();i++)
            {
                Point start,end;
                contours[i].getBoundingRectangle(start,end);
                double ratio = (double)(end.x-start.x)/(end.y-start.y);
                int area = (end.x-start.x)*(end.y-start.y);
                if((ratio > 10)&&(area>100))
                {
                    mast.push_back(contours[i]);
                }
                else
                {
                    if(area<15&&search)
                    {
                        small_wire.push_back(contours[i]);
                    }
                    else
                    {
                        wire.push_back(contours[i]);
                    }
                }
            }
            break;
        case WIRE_SECOND:
        //////// as above but with different conditions
            for(unsigned int i=0;i<contours.size();i++)
            {
                Point start,end;
                contours[i].getBoundingRectangle(start,end);
                double ratio = (double)(end.x-start.x)/(end.y-start.y);
                int area = (end.x-start.x)*(end.y-start.y);
                if((ratio > 10)&&(area>100))
                {
                    mast.push_back(contours[i]);
                }
                else
                {
                    if(area<15&&search)
                    {
                        small_wire.push_back(contours[i]);
                    }
                    else
                    {
                        wire.push_back(contours[i]);
                    }
                }
            }
            break;
        case MAST:
            break;
        case OBJECTS:
            break;
    }
    
}
bool getNewPosition(std::vector<ImageMatrixUchar> &images,Point &roi_origin,
                     Point &roi_range,std::vector<Point> &wire_points, std::ofstream &file)
{
    std::vector<Blob> contours;
    std::vector<std::vector<unsigned int> > contour_index;
    std::vector<unsigned int> probable_upper;
    std::vector<unsigned int> probable_lower;
    bool search = !setupImage(images,roi_origin,state);
    processImage(images[0],contours);
    separateContours(contours,contour_index,state);
    int flag=0;
    if(contour_index[1].size()==0)
    {
        Point center1;
        Point center2;
        if(search)
        {
            for(unsigned int i=0; i<contour_index[0].size(); i++)
            {
                contours[(contour_index[0])[i]].getCenter(center1);
                int center_outer_x = (int)center1.x;
                int center_outer_y = (int)center1.y;
                for(unsigned int j=i+1; j<contour_index[0].size(); j++)
                {
                    contours[(contour_index[0])[j]].getCenter(center2);
                    int center_inner_x = center2.x;
                    int center_inner_y = center2.y;
                    int delta_x = center_outer_x-center_inner_x;
                    int delta_y = center_outer_y-center_inner_y;
                    int computed_x = (center_outer_x+center_inner_x)/2;
                    int computed_y = (center_outer_y+center_inner_y)/2;
                    if(search||true)
                    {
                        if(((abs(delta_x))<15)&&(abs(delta_y)>30))
                        {
                            plotContoursFill(images[1],contours[(contour_index[0])[i]],255,255,255);
                            plotContoursFill(images[1],contours[(contour_index[0])[j]],255,255,255);
                            if(delta_y>=0)
                            {
                                wire_points[0].x = center1.x+roi_origin.x;
                                wire_points[0].y = center1.y+roi_origin.y;
                                wire_points[1].x = center2.x+roi_origin.x;
                                wire_points[1].y = center2.y+roi_origin.y;
                                first = center1;
                                second = center2;
                                //prepareRadonTransform(images[0],contours[(contour_index[0])[i]],contours[(contour_index[0])[j]],file);
                            }
                            else
                            {
                                wire_points[1].x = center1.x+roi_origin.x;
                                wire_points[1].y = center1.y+roi_origin.y;
                                wire_points[0].x = center2.x+roi_origin.x;
                                wire_points[0].y = center2.y+roi_origin.y;
                                first = center2;
                                second = center1;
                                //prepareRadonTransform(images[0],contours[(contour_index[0])[j]],contours[(contour_index[0])[j]],file);
                            }
                            current_x.x = (double)(center_outer_x+center_inner_x)/2.0;
                            previous_x.u = (double)(current_x.x-roi_range.x/2.0)/2.0;
                            previous_x.a = 0;
                            previous_y.x = (double)(center_outer_y+center_inner_y)/2.0;
                            flag =1;
                            state.setcurrentState(WIRE);
    /*                        if((center_outer_x+center_inner_x) < roi_range.x)
                                roi_origin.x = 0;
                            else
                                roi_origin.x=((center_outer_x+center_inner_x-roi_range.x)/2);
                            if((center_outer_y+center_inner_y) < roi_range.y)
                                roi_origin.y = 0;
                            else
                                roi_origin.y=((center_outer_y+center_inner_y-roi_range.y)/2);*/
                            break;
                        }
                    }
                    else
                    {
                        if(((abs(delta_x))<10)&&(abs(delta_y)>20)&&(computed_x>(previous_x_-10))
                                &&computed_x<(previous_x_+10)&&computed_y>(previous_y_-10)
                                &&computed_y<(previous_y_+10))
                        {
                            if(delta_y>=0)
                            {
                                probable_upper.push_back((contour_index[0])[i]);
                                probable_lower.push_back((contour_index[0])[j]);
                            }
                            else
                            {
                                probable_upper.push_back((contour_index[0])[j]);
                                probable_lower.push_back((contour_index[0])[i]);
                            }
                        }
                    }
                    if(flag==1&&search)
                    {
                        //printDebugMessage("inner Pair has been found switching to ROI mode");
                        break;
                    }
                }
                if(flag==1&&search)
                {
                    //printDebugMessage("Pair has been found switching to ROI mode");
                    break;
                }
            }
        }
        else
        {
            for(unsigned int i=0; i<contour_index[0].size(); i++)
            {
                contours[(contour_index[0])[i]].getCenter(center1);
                int temp1 = center1.x-first.x+roi_origin.x;
                int temp2 = center1.y-first.y+roi_origin.y;
                if((abs(temp1)<ROW_LIMIT)&&(abs(temp2)<COL_LIMIT))
                {
                    probable_upper.push_back((contour_index[0])[i]);
                }
                else
                {
                    temp1 = center1.x-second.x+roi_origin.x;
                    temp2 = center1.y-second.y+roi_origin.y;
                    if((abs(temp1)<ROW_LIMIT)&&(abs(temp2)<COL_LIMIT))
                    {
                        probable_lower.push_back((contour_index[0])[i]);
                    }   
                }
            }
            int upper_index,lower_index;
            if(findProbablePair(contours,probable_upper,probable_lower,upper_index,lower_index))
            {
                //prepareRadonTransform(images[0],contours[upper_index],
                                      //contours[lower_index],file);
                contours[upper_index].getCenter(center1);
                contours[lower_index].getCenter(center2);
                center2.x += roi_origin.x;
                center2.y += roi_origin.y;
                center1.x += roi_origin.x;
                center1.y += roi_origin.y;
                wire_points[0] = center1;
                wire_points[1] = center2;
                current_x.x = (double)(center1.x+center2.x)/2.0;
                current_x.u = (double)(current_x.x-previous_x.x);
                previous_x.a = (double)(current_x.u-previous_x.u);
                previous_y.x = (double)(center2.y+center1.y)/2.0;
                /*std::cout<<"estimated value "<< previous_x.x
                        <<" "<<previous_y.x<<std::endl;
                std::cout<<"found value "<< current_x.x
                        <<" "<<previous_y.x<<std::endl;*/
                flag = 1;
                plotContoursFill(images[1],contours[upper_index],255,255,255);
                plotContoursFill(images[1],contours[lower_index],255,255,255);
                first = center1;
                second = center2;
                state.setcurrentState(WIRE);
            }
            else
            {
                /*if(probable_upper.size()>0)
                {
                    flag = 1;
                    contours[upper_index].getCenter(center1);
                    center2.x = second.x+(center1.x-first.x);
                    center2.y = second.y;
                    center1.x += roi_origin.x;
                    center1.y += roi_origin.y;
                    wire_points[0] = center1;
                    wire_points[1] = center2;
                    current_x.x = (double)(center1.x+center2.x)/2.0;
                    current_x.u = (double)(current_x.x-previous_x.x);
                    previous_x.a = (double)(current_x.u-previous_x.u);
                    previous_y.x = (double)(center2.y+center1.y)/2.0;
                    flag = 1;
                    state.setcurrentState(WIRE_SECOND);
                }
                else
                {
                    if(probable_lower.size()>0)
                    {
                        contours[lower_index].getCenter(center2);
                        center2.x += roi_origin.x;
                        center2.y += roi_origin.y;
                        center1.x = first.x+(center2.x-second.x);
                        center1.y = first.y;
                        wire_points[0] = center1;
                        wire_points[1] = center2;
                        current_x.x = (double)(center1.x+center2.x)/2.0;
                        current_x.u = (double)(current_x.x-previous_x.x);
                        previous_x.a = (double)(current_x.u-previous_x.u);
                        previous_y.x = (double)(center2.y+center1.y)/2.0;
                        flag = 1;
                        state.setcurrentState(WIRE_SECOND);
                    }
                    else
                    {
                        flag = 0;
                    }
                }
                */
                flag =0;
            }
        }
    }
    else
    {
        //printErrorMessage("Mast has been found switching to normal mode");
        //printErrorMessage("Mast Mast Mast");
    }
    if(images.size()>2)
    {
        for(unsigned int i =0; i<contours.size();i++)
        {
            plotContoursFill(images[2],contours[i],0,255,0);
        }
    }
    if(images.size()>3&&false)
    {
        images[3].setROI(roi_origin,roi_range);
        for(unsigned int i =0; i<contour_index[1].size();i++)
        {
            //plotContoursFill(images[3],mast[i],255,0,0);
        }
        images[3].removeROI();
    }
    images[0].removeROI();
    images[1].removeROI();
    images[2].removeROI();
    if(flag==0)
    {
        //printErrorMessage("No contour has been found switching to normal mode");
        state.setcurrentState(START);
        return false;
    }
    else
    {
        return true;
    }
}

bool findProbablePair(std::vector<Blob> &contours,std::vector<unsigned int> &upper,
                            std::vector<unsigned int> &lower,int &upper_index,int &lower_index)
{
    upper_index =0;
    lower_index =0;
    long energy_min =10000000;
    Point center1,center2;
    for(unsigned int i =0;i<upper.size();i++)
    {
        contours[upper[i]].getCenter(center1);
        for(unsigned int j=0;j<lower.size();j++)
        {
            contours[lower[j]].getCenter(center2);
            long energy = pow((int)((center1.x+center2.x)/2.0-previous_x_),2)
                +pow((int)((center1.y+center2.y)/2.0-previous_y_),2);
            if(energy<energy_min)
            {
                energy_min = energy;
                upper_index = upper[i];
                lower_index = lower[j];
            }
        }
    }
    if((upper.size()>0)&&(lower.size()>0))
    {
        
        //std::cout<<"best match "<<upper_index<<" "<<lower_index<<" out of "<<upper.size()<<std::endl;
        return true;
    }
    //printErrorMessage("no match found");
    return false;
}

void setTimeGap(double value)
{
    time = value;
}

void resetCalulation()
{
    previous_x.reset();
    current_x.reset();
    previous_y.reset();
    current_y.reset();
}

} // end namespace ohe_framework


